wm = 10000/60*2*pi;  
%电机转速，单位一开始是rpm，所需要除以60再乘以2pi
I_prop = 0.000039337;
%桨叶转动惯量，单位是kg·m^2
I=[0.045 0 0;0 0.045 0;0 0 0.023];
%惯性张量，单位是kg·m^2
L1 = 72.79/1000;
%舵片升力点到轴中心垂直距离，单位m
L2 = 141.67/1000;
%舵片升力中心平面到质心的垂直距离，单位m
m = 2.541;
%飞行器重量，单位kg
T = 0.001;
%离散周期，单位s
n = 1000;
%仿真步数

Ad = [1 T*I_prop*wm/I(1,1) 0;-T*I_prop*wm/I(2,2) 1 0;0 0 1];
Bd = [T*L2/I(1,1) 0 0 0;0 0 T*L2/I(2,2) 0;0 T*L1/I(3,3) 0 -T*L1/I(3,3)];

A_b = [Ad,zeros(3,3),Bd;zeros(3,3),eye(3),zeros(3,4);zeros(4,3),zeros(4,3),eye(4)];
B_b = [Bd;zeros(3,4);eye(4)];

U = zeros(4,1);
p = zeros(n,1);
q = zeros(n,1);
r = zeros(n,1);
time = zeros(n,1);
q0 = [1;0;0;0];
roll = zeros(n,1);
pitc = zeros(n,1);
yaw = zeros(n,1);

X0 = [0.1;0.2;0];
Xd0 = [0.3;0;0];
u0 = [0;0;0;0];
Xb0 = [X0;Xd0;u0];
%系统初始状态向量
X = X0;
Xbk = Xb0;
uk = u0;
duk = zeros(4,1);
%状态向量Xk和增广状态向量
P=zeros(n,100);
%P 10x10
Cb = [eye(3),-eye(3),zeros(3,4)];
% Cb 3x10
P0 = Cb' * [1 0 0;0 1 0;0 0 1] * Cb;
%末端状态代价矩阵10x10
Q = Cb' * [1 0 0;0 1 0;0 0 1] * Cb;
%过程状态代价矩阵10x10
R = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
%过程输入代价矩阵4x4
K = zeros(n,40);
%全状态反馈矩阵 4x10
P(1,:) = P0(:)';
%初始化

for i = 2:n
    tmpP = reshape(P(i-1,:),10,10);   
    K(n-i+1,:) = reshape( (B_b'*tmpP*B_b+R)\B_b'*tmpP*A_b,1,40);
    tmpK = reshape(K(n-i+1,:),4,10);
    P(i,:)= reshape( (A_b-B_b * tmpK)'* tmpP *(A_b-B_b * tmpK) + tmpK'*R*tmpK+Q ,1,100);
end
%从最后一个往前算P(k)

for i = 1:n
    time(i) = (i-1)*T;
    p(i) = X(1);
    q(i) = X(2);
    r(i) = X(3);
    roll(i) = atan2(2*q0(1)*q0(2)+2*q0(3)*q0(4),q0(1)*q0(1)-q0(2)*q0(2)-q0(3)*q0(3)+q0(4)*q0(4)) / 2/pi *360;
    tmp = 2*q0(1)*q0(3)-2*q0(2)*q0(4);
    if tmp >= 1
        pitc(i) = pi/2;
    elseif tmp <= -1
        pitc(i) = -pi/2;
    else
        pitc(i) = -asin(tmp) / pi *180;
    end
    
    yaw(i) = atan2(2*q0(1)*q0(4)+2*q0(2)*q0(3),q0(1)*q0(1)+q0(2)*q0(2)-q0(3)*q0(3)-q0(4)*q0(4)) / 2/pi *360;    
    Cq = [1 -T*X(1) -T*X(2) -T*X(3);T*X(1) 1 T*X(3) -T*X(2);T*X(2) -T*X(3) 1 T*X(1);T*X(3) T*X(2) -T*X(1) 1];
    q0 = Cq * q0;
    
    Kmatrix = reshape(K(i,:),4,10);
    duk = - Kmatrix*Xbk;     
    uk = uk+duk;    
    X = Ad*X + Bd*uk;
    Xbk = A_b*Xbk + B_b*duk;  
end


plot_row = 2;
plot_column = 1;
subplot(plot_row,plot_column,1);
plot(time, p,'r',time, q,'g',time,r,'b') % 绘制曲线 
xlabel('t') % 添加x轴标签
ylabel('w') % 添加y轴标签
title('机体系角速度数据曲线') % 添加标题
legend('wx','wy','wz');
grid on % 添加网格线
subplot(plot_row,plot_column,2);
plot(time, roll,'r',time, pitc,'g',time,yaw,'b') % 绘制曲线 
xlabel('t') % 添加x轴标签
ylabel('a') % 添加y轴标签
title('欧拉角') % 添加标题
legend('roll','pitch','yaw');
grid on % 添加网格线

